#include <stdlib.h>
#include <cmath>
#include "MarkerTracker.h"
#include "ObjectDrawingFunctions.h"
#include "VirtualCameraCommunication.h"
#include "SimpleVectorMatrix.h"
#include "SnowmanFight.h"
#include "AugmentedObjects.h"
#include "FaceModel.h"

#include <GL/glut.h>
#include <GL/gl.h>
//#include <GL/glext.h>		//in this include file the #include <GL/gl.h> is already included


const int DISPLAY_WIDTH = 640;
const int DISPLAY_HEIGHT = 480;

const int CALIBRATE_FRAMES_NUMBER = 20;

const short SIMPLE_SNOWMAN_ID = 0x005a;
//const short FAT_SNOWMAN_ID = 0x0272;
const short RED_NOSE_ID = 0x1c44;	

const short CAMERA_MARKER = 0x1c44;

unsigned char bkgnd[DISPLAY_WIDTH * DISPLAY_HEIGHT * 3];

std::map<short, ObjectDrawFunction> markerObjectMap;

void initMarkerObjects() {
	//markerObjectMap[SIMPLE_SNOWMAN_ID] = drawSimpleSnowman;
	//markerObjectMap[FAT_SNOWMAN_ID] = drawFatSnowman;
	markerObjectMap[RED_NOSE_ID] = drawRedNose;
}

void displayKnownMarkerObjects()
{
	std::map<short, ObjectDrawFunction>::iterator marker_iter;

	for (marker_iter = markerObjectMap.begin(); marker_iter != markerObjectMap.end(); ++marker_iter)
	{
		std::map<short, float*>::iterator element = markers_map.find(marker_iter->first);
		if (element != markers_map.end())
			displayObjectOnMarker(element->second, marker_iter->second);
	}
}


void project3Dpoint(double* point)
{
	GLdouble proj_matrix[16];				
	glGetDoublev(GL_PROJECTION_MATRIX, proj_matrix);	
	
	GLdouble mod_matrix[16];				
	glGetDoublev(GL_MODELVIEW_MATRIX, mod_matrix);	

	GLint view_matrix[4];
	glGetIntegerv(GL_VIEWPORT, view_matrix);

	GLdouble res[4];
	gluProject(0, 0, 0, mod_matrix, proj_matrix, view_matrix, res, res+1, res+2);;

	//printf("CENTER = (%d,%d)\n", (int) res[0], (int) res[1]);
}



void display_callback()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();
			
	// Draw background
	glDisable( GL_DEPTH_TEST );
	glMatrixMode( GL_PROJECTION );
	glPushMatrix();
	glLoadIdentity();
	gluOrtho2D( 0.0, DISPLAY_WIDTH, 0.0, DISPLAY_HEIGHT );
	glRasterPos2i( 0, DISPLAY_HEIGHT-1 );
	glDrawPixels( DISPLAY_WIDTH, DISPLAY_HEIGHT, 0x80E0, GL_UNSIGNED_BYTE, bkgnd ); //GL_BGR_EXT replaced with 0x80E0
	glPopMatrix();
	glEnable(GL_DEPTH_TEST);

	std::map<short, float*>::iterator marker = markers_map.find(CAMERA_MARKER);
	if (marker != markers_map.end())
	{
		displayActiveModels(marker->second);
		double center[] = {marker->second[3], marker->second[7], marker->second[11], marker->second[15]};
		
		if (calibratingAdditionalMarkers)
		{
			faceModel.displayModelPoints();
			faceModel.updateModel();
			glLoadIdentity();
			glTranslatef(-2, 1, -3);
			glutSolidSphere(calibratingAdditionalMarkers/100.0f, 20, 20);
		}
	}



	glutPostRedisplay();

	glutSwapBuffers();
	
	sendPixelsToSharedMemory(DISPLAY_WIDTH, DISPLAY_HEIGHT);
}

void reshape_callback(int width, int height)
{
	glViewport(0, 0, width, height);

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(60, ((double)DISPLAY_WIDTH/(double)DISPLAY_HEIGHT), 0.01, 100);
}

void idle_callback()
{
	mainLoop();
	memcpy( bkgnd, frame_image->imageData, sizeof(bkgnd) );
}

#ifndef _TCHAR
#define _TCHAR char
#endif

void keyUpCallback (int key, int x, int y) {
	if (key == GLUT_KEY_UP) {
		//rotate around positive x-axis
		plane_x_angle += 0.1;
	} else if (key == GLUT_KEY_DOWN) {
		//rotate around positive x-axis
		plane_x_angle -= 0.1;
	}
}


void menu_callback(int id)
{
	if (id < activeObjects.size())
		activeObjects[id] = !activeObjects[id];
	else if (id == activeObjects.size() + 1)
		hideMarker = !hideMarker;
	else if (id == activeObjects.size() + 3) {
		if (!planeCalibEnabled)
			glutSpecialUpFunc(keyUpCallback);
		else
			glutSpecialUpFunc(NULL);
		planeCalibEnabled = !planeCalibEnabled;
	}
	else if (id == activeObjects.size() + 4)
	{
		calibratingAdditionalMarkers = CALIBRATE_FRAMES_NUMBER;
		faceModel.clearModel();
	}
}

void initMenu()
{
	int menu = glutCreateMenu(menu_callback);

	for (int i=0; i<objectNames.size(); ++i)
		glutAddMenuEntry(objectNames[i]->c_str(), i);

	glutAddMenuEntry("", objectNames.size());
	glutAddMenuEntry("Show/Hide Marker", objectNames.size()+1);

	glutAddMenuEntry("", objectNames.size()+2);
	glutAddMenuEntry("Calibrate Marker Plane", objectNames.size()+3);
	glutAddMenuEntry("Calibrate Face Model", objectNames.size()+4);

	glutAttachMenu(GLUT_RIGHT_BUTTON);
}


int main(int argc, _TCHAR* argv[])
{
	initModels();	
	//initMarkerObjects();

	glutInit(&argc, (char**)(&argv));
	glutInitWindowSize(DISPLAY_WIDTH, DISPLAY_HEIGHT);
	glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH);

	glutCreateWindow("CamARa");

	initMenu();

    glClearColor( 0.5, 0.5, 1.0, 1.0 );

	// Pixel storage/packing stuff
	glPixelStorei( GL_PACK_ALIGNMENT, 1 );
	glPixelStorei( GL_UNPACK_ALIGNMENT, 1 );
	glPixelZoom( 1.0, -1.0 );

	glEnable(GL_DEPTH_TEST);
	glClearDepth(1.0);
	
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);

	GLfloat light_pos[] = { -1.5f, 1.0f, 2.0f, 1.0f };
	GLfloat light_amb[] = { 0.02f, 0.02f, 0.02f, 1.0f };
	GLfloat light_dif[] = { 0.08f, 0.08f, 0.08f, 1.0f };
	GLfloat light_spec[] = { 0.5f, 0.5f, 0.5f, 1.0f };

	glLightfv( GL_LIGHT0, GL_POSITION, light_pos );
	glLightfv( GL_LIGHT0, GL_AMBIENT,  light_amb );
	glLightfv( GL_LIGHT0, GL_DIFFUSE,  light_dif );
	glLightfv( GL_LIGHT0, GL_SPECULAR,  light_spec );

	

	

	glutDisplayFunc(display_callback);
	glutReshapeFunc(reshape_callback);
	glutIdleFunc(idle_callback);

	init();

	glutMainLoop();

	finish();

	freeSharedMemory();

	freeModels();

	return 0;
}

